<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>src/qvsfm/qvsfm.cpp</h1><a href="qvsfm_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*</span>
<a name="l00002"></a>00002 <span class="comment"> *      Copyright (C) 2011, 2012. PARP Research Group.</span>
<a name="l00003"></a>00003 <span class="comment"> *      &lt;http://perception.inf.um.es&gt;</span>
<a name="l00004"></a>00004 <span class="comment"> *      University of Murcia, Spain.</span>
<a name="l00005"></a>00005 <span class="comment"> *</span>
<a name="l00006"></a>00006 <span class="comment"> *      This file is part of the QVision library.</span>
<a name="l00007"></a>00007 <span class="comment"> *</span>
<a name="l00008"></a>00008 <span class="comment"> *      QVision is free software: you can redistribute it and/or modify</span>
<a name="l00009"></a>00009 <span class="comment"> *      it under the terms of the GNU Lesser General Public License as</span>
<a name="l00010"></a>00010 <span class="comment"> *      published by the Free Software Foundation, version 3 of the License.</span>
<a name="l00011"></a>00011 <span class="comment"> *</span>
<a name="l00012"></a>00012 <span class="comment"> *      QVision is distributed in the hope that it will be useful,</span>
<a name="l00013"></a>00013 <span class="comment"> *      but WITHOUT ANY WARRANTY; without even the implied warranty of</span>
<a name="l00014"></a>00014 <span class="comment"> *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span>
<a name="l00015"></a>00015 <span class="comment"> *      GNU Lesser General Public License for more details.</span>
<a name="l00016"></a>00016 <span class="comment"> *</span>
<a name="l00017"></a>00017 <span class="comment"> *      You should have received a copy of the GNU Lesser General Public</span>
<a name="l00018"></a>00018 <span class="comment"> *      License along with QVision. If not, see &lt;http://www.gnu.org/licenses/&gt;.</span>
<a name="l00019"></a>00019 <span class="comment"> */</span>
<a name="l00020"></a>00020 
<a name="l00024"></a>00024 
<a name="l00025"></a>00025 <span class="preprocessor">#include &lt;qvsfm.h&gt;</span>
<a name="l00026"></a>00026 <span class="preprocessor">#include &lt;QSet&gt;</span>
<a name="l00027"></a>00027 
<a name="l00028"></a>00028 <span class="preprocessor">#ifndef DOXYGEN_IGNORE_THIS</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="keywordtype">double</span> reconstructionError(<span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Rt1, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Rt2, <span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp;matchings)
<a name="l00030"></a>00030         {
<a name="l00031"></a>00031         QList&lt;QV3DPointF&gt; points3D;
<a name="l00032"></a>00032         <span class="keywordflow">foreach</span>(<a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a> matching, matchings)
<a name="l00033"></a>00033                 points3D &lt;&lt; <a class="code" href="group__qvprojectivegeometry.html#ga1d52c9946346c83aea79f1f2311b44ef" title="Recovers the location of a 3D point from its projection on several views, and their...">linear3DPointTriangulation</a>(matching, Rt1, Rt2);
<a name="l00034"></a>00034 
<a name="l00035"></a>00035         <span class="keywordflow">return</span> reconstructionError(Rt1, Rt2, points3D, matchings);
<a name="l00036"></a>00036         }
<a name="l00037"></a>00037 <span class="preprocessor">#endif // DOXYGEN_IGNORE_THIS</span>
<a name="l00038"></a>00038 <span class="preprocessor"></span>
<a name="l00039"></a><a class="code" href="group__qvsfm.html#gaab6ac97055fa83864cb3bcad712008b7">00039</a> <span class="keywordtype">bool</span> <a class="code" href="group__qvsfm.html#gaab6ac97055fa83864cb3bcad712008b7" title="Initialize the projection matrices of two views in a reconstruction, provided a list...">linearCameraPairInitialization</a>(<span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp;matchings, <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Rt1, <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Rt2)
<a name="l00040"></a>00040         {
<a name="l00041"></a>00041         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> E;
<a name="l00042"></a>00042         <span class="keywordflow">if</span> (not <a class="code" href="group__qvprojectivegeometry.html#ga721c81d3a9e9c7c2e09f5867241c7021" title="Obtains the fundamental matrix between two images using the 8-point algorithm.">computeFundamentalMatrix</a>(matchings, E))
<a name="l00043"></a>00043                 {
<a name="l00044"></a>00044                 <span class="comment">/*#ifdef DEBUG</span>
<a name="l00045"></a>00045 <span class="comment">                std::cout &lt;&lt; &quot;[linearCameraPairInitialization] ERROR: could not compute essential matrix.&quot; &lt;&lt; std::endl; </span>
<a name="l00046"></a>00046 <span class="comment">                #endif // DEBUG*/</span>
<a name="l00047"></a>00047                 <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00048"></a>00048                 }
<a name="l00049"></a>00049 
<a name="l00050"></a>00050         <span class="comment">// Get camera poses from the epipolar geometry.</span>
<a name="l00051"></a>00051         <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> R1, R2;
<a name="l00052"></a>00052         <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> t;
<a name="l00053"></a>00053         <a class="code" href="group__qvprojectivegeometry.html#ga017edd807b040c198a02733e4b81e1df" title="Decomposes an essential matrix and obtains the corresponding pair of camera poses...">getCameraPosesFromEssentialMatrix</a>(E, R1, R2, t);
<a name="l00054"></a>00054 
<a name="l00055"></a>00055         <span class="comment">/*#ifdef DEBUG</span>
<a name="l00056"></a>00056 <span class="comment">        QVMatrix U, V;</span>
<a name="l00057"></a>00057 <span class="comment">        QVVector sv;</span>
<a name="l00058"></a>00058 <span class="comment">        singularValueDecomposition(E, U, sv, V);</span>
<a name="l00059"></a>00059 <span class="comment">        std::cout &lt;&lt; &quot;[linearCameraPairInitialization] s2 for essential matrix = &quot; &lt;&lt; (sv[1]/sv[0]) &lt;&lt; std::endl; </span>
<a name="l00060"></a>00060 <span class="comment">        #endif // DEBUG*/</span>
<a name="l00061"></a>00061 
<a name="l00062"></a>00062         Q_ASSERT(not R1.<a class="code" href="classQVMatrix.html#a43455dfaf2e8fa71242ea1fdc8beb102" title="Checks whether the matrix contains a NaN value or not.">containsNaN</a>());
<a name="l00063"></a>00063         Q_ASSERT(not R2.<a class="code" href="classQVMatrix.html#a43455dfaf2e8fa71242ea1fdc8beb102" title="Checks whether the matrix contains a NaN value or not.">containsNaN</a>());
<a name="l00064"></a>00064         Q_ASSERT(not t.<a class="code" href="classQVVector.html#a29235517605f49f2327df62d5e0295df" title="Checks whether the vector contains a NaN value or not.">containsNaN</a>());
<a name="l00065"></a>00065 
<a name="l00066"></a>00066         <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a>  sourceRt = (<a class="code" href="classQVMatrix.html#a6eb15980d17c17d4a55ab09a77bcd715" title="Creates an identity matrix.">QVMatrix::identity</a>(3)|<a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a>(3,0.0)),
<a name="l00067"></a>00067                                         destRt1 = R1|t,
<a name="l00068"></a>00068                                         destRt2 = R1|t*(-1.0),
<a name="l00069"></a>00069                                         destRt3 = R2|t,
<a name="l00070"></a>00070                                         destRt4 = R2|t*(-1.0);
<a name="l00071"></a>00071 
<a name="l00072"></a>00072         <span class="comment">// Test for significant 3D structure</span>
<a name="l00073"></a>00073         <span class="comment">/*#ifdef DEBUG</span>
<a name="l00074"></a>00074 <span class="comment">        const double    errorCorrect = reconstructionError(sourceRt, destRt1, matchings),</span>
<a name="l00075"></a>00075 <span class="comment">                                        errorIncorrect = reconstructionError(destRt1, sourceRt, matchings);</span>
<a name="l00076"></a>00076 <span class="comment"></span>
<a name="l00077"></a>00077 <span class="comment">        std::cout &lt;&lt; &quot;[linearCameraPairInitialization] Reconstruction errors: &quot; &lt;&lt; errorCorrect &lt;&lt; &quot;\t&quot; &lt;&lt; errorIncorrect &lt;&lt; std::endl; </span>
<a name="l00078"></a>00078 <span class="comment">        #endif // DEBUG*/</span>
<a name="l00079"></a>00079 
<a name="l00080"></a>00080         <span class="comment">// Test cheirality for the different possible destination camera poses using the first point correspondence.</span>
<a name="l00081"></a>00081         <span class="keyword">const</span> <a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a> matching = matchings[0];
<a name="l00082"></a>00082 
<a name="l00083"></a>00083         <span class="keywordtype">int</span> R1Correct = 0, R2Correct = 0, tPossitive = 0, tNegative = 0;
<a name="l00084"></a>00084         <span class="comment">//bool R1IsCorrect, tIsPossitive;</span>
<a name="l00085"></a>00085 
<a name="l00086"></a>00086         <span class="keywordflow">foreach</span>(<a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a> matching, matchings)
<a name="l00087"></a>00087                 {
<a name="l00088"></a>00088                 <span class="keyword">const</span> <span class="keywordtype">bool</span>      <span class="comment">// for camera poses (I|0) and (R1|t)</span>
<a name="l00089"></a>00089                                 cheiralityTest1 = <a class="code" href="group__qvprojectivegeometry.html#ga5b7c05c6c12bb5c005f00cd838fd436b" title="Tests if two camera poses satisfy the cheirality condition for the reconstruction...">testCheiralityForCameraPoses</a>(sourceRt, matching.first, destRt1, matching.second ),
<a name="l00090"></a>00090                                 <span class="comment">// for camera poses (I|0) and (R1|-t)</span>
<a name="l00091"></a>00091                                 cheiralityTest2 = <a class="code" href="group__qvprojectivegeometry.html#ga5b7c05c6c12bb5c005f00cd838fd436b" title="Tests if two camera poses satisfy the cheirality condition for the reconstruction...">testCheiralityForCameraPoses</a>(sourceRt, matching.first, destRt2, matching.second ),
<a name="l00092"></a>00092                                 <span class="comment">// for camera poses (I|0) and (R2|t)</span>
<a name="l00093"></a>00093                                 cheiralityTest3 = <a class="code" href="group__qvprojectivegeometry.html#ga5b7c05c6c12bb5c005f00cd838fd436b" title="Tests if two camera poses satisfy the cheirality condition for the reconstruction...">testCheiralityForCameraPoses</a>(sourceRt, matching.first, destRt3, matching.second ),
<a name="l00094"></a>00094                                 <span class="comment">// for camera poses (I|0) and (R2|-t)</span>
<a name="l00095"></a>00095                                 cheiralityTest4 = <a class="code" href="group__qvprojectivegeometry.html#ga5b7c05c6c12bb5c005f00cd838fd436b" title="Tests if two camera poses satisfy the cheirality condition for the reconstruction...">testCheiralityForCameraPoses</a>(sourceRt, matching.first, destRt4, matching.second );
<a name="l00096"></a>00096 
<a name="l00097"></a>00097                 <span class="comment">// Test if there is more than one configuration with the correct cheirality.</span>
<a name="l00098"></a>00098                 <span class="keywordflow">if</span> (cheiralityTest1 + cheiralityTest2 +  cheiralityTest3 + cheiralityTest4 != 1)
<a name="l00099"></a>00099                         {
<a name="l00100"></a>00100                         <span class="comment">/*#ifdef DEBUG</span>
<a name="l00101"></a>00101 <span class="comment">                        std::cout &lt;&lt; &quot;[linearCameraPairInitialization] ERROR: number of camera pose combinations with correct cheirality is &quot;  &lt;&lt; (cheiralityTest1 + cheiralityTest2 +  cheiralityTest3 + cheiralityTest4) &lt;&lt; std::endl; </span>
<a name="l00102"></a>00102 <span class="comment">                        #endif // DEBUG*/</span>
<a name="l00103"></a>00103                         <span class="keywordflow">return</span> <span class="keyword">false</span>;
<a name="l00104"></a>00104                         }
<a name="l00105"></a>00105 
<a name="l00106"></a>00106                 <span class="comment">// Set the destination camera pose depending on which of them satisfies the cheirality test.</span>
<a name="l00107"></a>00107                 <span class="keywordflow">if</span> (cheiralityTest1)
<a name="l00108"></a>00108                         <span class="comment">// Cheirality is satisfied only for camera poses (I|0) and (R1|t)</span>
<a name="l00109"></a>00109                         { R1Correct ++; tPossitive++; }
<a name="l00110"></a>00110                 <span class="keywordflow">else</span> <span class="keywordflow">if</span> (cheiralityTest2)
<a name="l00111"></a>00111                         <span class="comment">// Cheirality is satisfied only for camera poses (I|0) and (R1|-t)</span>
<a name="l00112"></a>00112                         { R1Correct ++; tNegative++;  }
<a name="l00113"></a>00113                 <span class="keywordflow">else</span> <span class="keywordflow">if</span> (cheiralityTest3)
<a name="l00114"></a>00114                         <span class="comment">// Cheirality is satisfied only for camera poses (I|0) and (R2|t)</span>
<a name="l00115"></a>00115                         { R2Correct ++; tPossitive++;  }
<a name="l00116"></a>00116                 <span class="keywordflow">else</span> <span class="keywordflow">if</span> (cheiralityTest4)
<a name="l00117"></a>00117                         <span class="comment">// Cheirality is satisfied only for camera poses (I|0) and (R2|-t)</span>
<a name="l00118"></a>00118                         { R2Correct ++; tNegative++; }
<a name="l00119"></a>00119                 }
<a name="l00120"></a>00120 
<a name="l00121"></a>00121         <span class="keywordflow">if</span> ( (MIN(R1Correct, R2Correct) &gt; 0) or (MIN(tPossitive, tNegative) &gt; 0) )
<a name="l00122"></a>00122                 {
<a name="l00123"></a>00123                 <span class="comment">/*#ifdef DEBUG</span>
<a name="l00124"></a>00124 <span class="comment">                std::cout &lt;&lt; &quot;[linearCameraPairInitialization] WARNING: points with different cheirality found.&quot; &lt;&lt; std::endl; </span>
<a name="l00125"></a>00125 <span class="comment">                std::cout &lt;&lt; &quot;[linearCameraPairInitialization]\t|R1 correct| =\t&quot; &lt;&lt; R1Correct &lt;&lt; std::endl;</span>
<a name="l00126"></a>00126 <span class="comment">                std::cout &lt;&lt; &quot;[linearCameraPairInitialization]\t|R2 correct| =\t&quot; &lt;&lt; R2Correct &lt;&lt; std::endl;  </span>
<a name="l00127"></a>00127 <span class="comment">                std::cout &lt;&lt; &quot;[linearCameraPairInitialization]\t|t possitive| =\t&quot; &lt;&lt; tPossitive &lt;&lt; std::endl;</span>
<a name="l00128"></a>00128 <span class="comment">                std::cout &lt;&lt; &quot;[linearCameraPairInitialization]\t|t negative| =\t&quot; &lt;&lt; tNegative &lt;&lt; std::endl;  </span>
<a name="l00129"></a>00129 <span class="comment">                #endif // DEBUG*/</span>
<a name="l00130"></a>00130                 <span class="comment">//return false;</span>
<a name="l00131"></a>00131                 }
<a name="l00132"></a>00132 
<a name="l00133"></a>00133         <span class="keyword">const</span> <span class="keywordtype">bool</span>      R1IsCorrect = (R1Correct &gt; R2Correct),
<a name="l00134"></a>00134                                 tIsPossitive = (tPossitive &gt; tNegative);
<a name="l00135"></a>00135 
<a name="l00136"></a>00136         <span class="comment">// Init camera poses.</span>
<a name="l00137"></a>00137         Rt1 = <a class="code" href="classQVMatrix.html#a6eb15980d17c17d4a55ab09a77bcd715" title="Creates an identity matrix.">QVMatrix::identity</a>(3)|<a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(0.0, 0.0, 0.0);
<a name="l00138"></a>00138         Rt2 = (R1IsCorrect?R1:R2)|(tIsPossitive?t:(t*-1.0));
<a name="l00139"></a>00139 
<a name="l00140"></a>00140         <span class="keywordflow">return</span> <span class="keyword">true</span>;
<a name="l00141"></a>00141         }
<a name="l00142"></a>00142 
<a name="l00143"></a><a class="code" href="group__qvsfm.html#ga88e5c045a99ad4e58d312ccee761860d">00143</a> QList&lt; QHash&lt; int, QPointF&gt; &gt; correctIntrinsics(<span class="keyword">const</span> QList&lt; QVMatrix &gt; &amp;Ks, <span class="keyword">const</span> QList&lt; QHash&lt; int, QPointF&gt; &gt; &amp;pointsProjections)
<a name="l00144"></a>00144         {
<a name="l00145"></a>00145         QList&lt;QVMatrix&gt; KsInv;
<a name="l00146"></a>00146         <span class="keywordflow">foreach</span>(<a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> K, Ks)
<a name="l00147"></a>00147                 KsInv &lt;&lt; <a class="code" href="group__qvmatrixalgebra.html#gadf6472ec782375d465ff1e87b3ae2557" title="Obtains the Moore–Penrose pseudoinverse of a matrix.">pseudoInverse</a>(K);
<a name="l00148"></a>00148 
<a name="l00149"></a>00149         QList&lt; QHash&lt; int, QPointF&gt; &gt; result;
<a name="l00150"></a>00150 
<a name="l00151"></a>00151         QHash&lt; int, QPointF&gt; pointProjections;
<a name="l00152"></a>00152         <span class="keywordflow">foreach</span>(pointProjections, pointsProjections)
<a name="l00153"></a>00153                 {
<a name="l00154"></a>00154                 QHash&lt; int, QPointF&gt; correctedPointProjections;
<a name="l00155"></a>00155                 <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> numCamera, pointProjections.keys())
<a name="l00156"></a>00156                         correctedPointProjections[numCamera] = <a class="code" href="group__qvprojectivegeometry.html#ga305b04da4d0035c3b4de07ff381b9e1c" title="Maps a point using an homography.">applyHomography</a>(KsInv[MIN(Ks.count()-1, numCamera)], pointProjections[numCamera]);
<a name="l00157"></a>00157                 result &lt;&lt; correctedPointProjections;
<a name="l00158"></a>00158                 }
<a name="l00159"></a>00159 
<a name="l00160"></a>00160         <span class="keywordflow">return</span> result;
<a name="l00161"></a>00161         }
<a name="l00162"></a>00162 
<a name="l00163"></a><a class="code" href="group__qvsfm.html#gafea767e25db8e80b1822b8a1499776fa">00163</a> <span class="keywordtype">bool</span> <a class="code" href="group__qvsfm.html#gafea767e25db8e80b1822b8a1499776fa" title="Test correct camera cheirality.">testCheirality</a>(<span class="keyword">const</span> QList&lt;QVCameraPose&gt; cameraPoses, <span class="keyword">const</span> QList&lt; QHash&lt; int, QPointF&gt; &gt; calibratedPointsProjections)
<a name="l00164"></a>00164         {
<a name="l00165"></a>00165         <span class="comment">// Get the correct sign for the camera centers with the cheirality condition.</span>
<a name="l00166"></a>00166         <span class="keyword">const</span> QList&lt;int&gt; cameraIndexesForPoint1 = calibratedPointsProjections.first().keys();
<a name="l00167"></a>00167         <span class="keyword">const</span> <span class="keywordtype">int</span>       frame1 = cameraIndexesForPoint1[0],
<a name="l00168"></a>00168                         frame2 = cameraIndexesForPoint1[1];
<a name="l00169"></a>00169                         
<a name="l00170"></a>00170         <span class="comment">// Compose the new camera poses from the optimal rotations and the estimated camera centers.</span>
<a name="l00171"></a>00171         <span class="keyword">const</span> <a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a> cameraPose0 = cameraPoses[frame1], cameraPose1 = cameraPoses[frame2];
<a name="l00172"></a>00172 
<a name="l00173"></a>00173         <span class="keyword">const</span> <span class="keywordtype">bool</span> cheiralityTest = <a class="code" href="group__qvprojectivegeometry.html#ga5b7c05c6c12bb5c005f00cd838fd436b" title="Tests if two camera poses satisfy the cheirality condition for the reconstruction...">testCheiralityForCameraPoses</a>(       cameraPose0.<a class="code" href="classQVCameraPose.html#ad976cef1f2d772209dfb55f0f105072b" title="Cast to a pin-hole projection matrix.">toProjectionMatrix</a>().<a class="code" href="classQVMatrix.html#a77b34335086bb9e12d7d72398c608e75" title="Gets a submatrix from a matrix.">getSubmatrix</a>(0,2,0,3), calibratedPointsProjections[0][frame1],
<a name="l00174"></a>00174                                                                         cameraPose1.toProjectionMatrix().<a class="code" href="classQVMatrix.html#a77b34335086bb9e12d7d72398c608e75" title="Gets a submatrix from a matrix.">getSubmatrix</a>(0,2,0,3), calibratedPointsProjections[0][frame2]);
<a name="l00175"></a>00175         <span class="keywordflow">return</span> cheiralityTest;
<a name="l00176"></a>00176         }
<a name="l00177"></a>00177 
<a name="l00178"></a><a class="code" href="group__qvsfm.html#gafe66d3df96f6c69ab795e2a070865dad">00178</a> <span class="keywordtype">void</span> <a class="code" href="group__qvsfm.html#gafe66d3df96f6c69ab795e2a070865dad" title="Swaps the cheirality of the camera poses and 3D points contained in a SfM reconstruction...">invertCheirality</a>(QList&lt;QVCameraPose&gt; &amp;cameraPoses, QList&lt;QV3DPointF&gt; &amp;points3D)
<a name="l00179"></a>00179         {
<a name="l00180"></a>00180         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; cameraPoses.count(); i++)
<a name="l00181"></a>00181                 cameraPoses[i] = <a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a>(cameraPoses[i].getOrientation(), -cameraPoses[i].getCenter());
<a name="l00182"></a>00182 
<a name="l00183"></a>00183         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; points3D.count(); i++)
<a name="l00184"></a>00184                 points3D[i] = -points3D[i];
<a name="l00185"></a>00185         }
<a name="l00186"></a>00186 
<a name="l00187"></a>00187 <span class="keywordtype">double</span> reconstructionError(     <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses, <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointProjections, <span class="keyword">const</span> QVector&lt;bool&gt; &amp;evaluateTracking)
<a name="l00188"></a>00188         {
<a name="l00189"></a>00189         <span class="keywordflow">return</span> reconstructionError(cameraPoses, <a class="code" href="group__qvprojectivegeometry.html#ga145fc6ee66fb884fa959a00400744570" title="Recovers the location of several 3D points from their projections on different views...">linear3DPointsTriangulation</a>(cameraPoses, pointProjections), pointProjections, evaluateTracking);
<a name="l00190"></a>00190         }
<a name="l00191"></a>00191 
<a name="l00192"></a><a class="code" href="group__qvsfm.html#ga24c3d362f20077a851e8539e4bc73aa4">00192</a> <span class="keywordtype">double</span> reconstructionError(     <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses, <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointProjections)
<a name="l00193"></a>00193         {
<a name="l00194"></a>00194         <span class="keywordflow">return</span> reconstructionError(cameraPoses, <a class="code" href="group__qvprojectivegeometry.html#ga145fc6ee66fb884fa959a00400744570" title="Recovers the location of several 3D points from their projections on different views...">linear3DPointsTriangulation</a>(cameraPoses, pointProjections), pointProjections);
<a name="l00195"></a>00195         }
<a name="l00196"></a>00196 
<a name="l00197"></a>00197 <span class="preprocessor">#ifdef DEBUG</span>
<a name="l00198"></a>00198 <span class="preprocessor"></span><span class="keywordtype">double</span> reconstructionError2(    <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses,
<a name="l00199"></a>00199                                                                 <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D,
<a name="l00200"></a>00200                                                                 <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointProjections)
<a name="l00201"></a>00201         {
<a name="l00202"></a>00202         <span class="keywordtype">int</span> count = 0;
<a name="l00203"></a>00203         <span class="keywordtype">double</span> error = 0.0;
<a name="l00204"></a>00204         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; pointProjections.size(); i++)
<a name="l00205"></a>00205                 <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, pointProjections[i].keys())
<a name="l00206"></a>00206                         {
<a name="l00207"></a>00207                         <span class="keyword">const</span> QPointF p = pointProjections[i][j] -  cameraPoses[j].project(points3D[i]);
<a name="l00208"></a>00208 
<a name="l00209"></a>00209                         error += p.x()*p.x() + p.y()*p.y();
<a name="l00210"></a>00210                         count+=2;
<a name="l00211"></a>00211                         }
<a name="l00212"></a>00212 
<a name="l00213"></a>00213         <span class="keywordflow">return</span> sqrt(error / <span class="keywordtype">double</span>(count));
<a name="l00214"></a>00214         }
<a name="l00215"></a>00215 <span class="preprocessor">#endif // DEBUG</span>
<a name="l00216"></a>00216 <span class="preprocessor"></span>
<a name="l00217"></a>00217 <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> squaredReprojectionErrorResidualsNew(  <span class="keyword">const</span> <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> &amp;point3D,
<a name="l00218"></a>00218                                                                                                 <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses,
<a name="l00219"></a>00219                                                                                                 <span class="keyword">const</span> QHash&lt;int, QPointF&gt; &amp;pointProjections)
<a name="l00220"></a>00220         {
<a name="l00221"></a>00221         <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> result;
<a name="l00222"></a>00222         result.reserve(pointProjections.count() * 2);
<a name="l00223"></a>00223 
<a name="l00224"></a>00224         <span class="keywordtype">int</span> count = 0;
<a name="l00225"></a>00225         <span class="keywordtype">double</span> error = 0.0;
<a name="l00226"></a>00226         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, pointProjections.keys())
<a name="l00227"></a>00227                 {
<a name="l00228"></a>00228                 <span class="keyword">const</span> QPointF p = pointProjections[j] -  cameraPoses[j].project(point3D);
<a name="l00229"></a>00229 
<a name="l00230"></a>00230                 result &lt;&lt; p.x()*p.x();
<a name="l00231"></a>00231                 result &lt;&lt; p.y()*p.y();
<a name="l00232"></a>00232                 }
<a name="l00233"></a>00233 
<a name="l00234"></a>00234         Q_ASSERT(result.count() == 2 * pointProjections.count() );
<a name="l00235"></a>00235 
<a name="l00236"></a>00236         <span class="keywordflow">return</span> result;<span class="comment">// / double(count);</span>
<a name="l00237"></a>00237         }
<a name="l00238"></a>00238 
<a name="l00239"></a>00239 <span class="keywordtype">double</span> squaredReprojectionErrorResiduals(       <span class="keyword">const</span> <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> &amp;point3D,
<a name="l00240"></a>00240                                                                                         <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses,
<a name="l00241"></a>00241                                                                                         <span class="keyword">const</span> QHash&lt;int, QPointF&gt; &amp;pointProjections)
<a name="l00242"></a>00242         {
<a name="l00243"></a>00243         <span class="keywordtype">int</span> count = 0;
<a name="l00244"></a>00244         <span class="keywordtype">double</span> error = 0.0;
<a name="l00245"></a>00245         <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, pointProjections.keys())
<a name="l00246"></a>00246                 {
<a name="l00247"></a>00247                 <span class="keyword">const</span> QPointF p = pointProjections[j] -  cameraPoses[j].project(point3D);
<a name="l00248"></a>00248 
<a name="l00249"></a>00249                 error += p.x()*p.x() + p.y()*p.y();
<a name="l00250"></a>00250                 count+=2;
<a name="l00251"></a>00251                 }
<a name="l00252"></a>00252 
<a name="l00253"></a>00253         Q_ASSERT(count == 2 * pointProjections.count() );
<a name="l00254"></a>00254 
<a name="l00255"></a>00255         <span class="keywordflow">return</span> error;<span class="comment">// / double(count);</span>
<a name="l00256"></a>00256         }
<a name="l00257"></a>00257 
<a name="l00258"></a>00258 <span class="keywordtype">double</span> squaredReprojectionErrorResiduals(       <span class="keyword">const</span> <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> &amp;point3D,
<a name="l00259"></a>00259                                                                                         <span class="keyword">const</span> <a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a> &amp;cameraPose,
<a name="l00260"></a>00260                                                                                         <span class="keyword">const</span> QPointF &amp;pointProjection)
<a name="l00261"></a>00261         {
<a name="l00262"></a>00262         <span class="keyword">const</span> QPointF p = pointProjection -  cameraPose.<a class="code" href="classQVCameraPose.html#a618a5bf0fe0bb41ba717eef051198a4b" title="Projects a 3D point at the image plane for the camera pose.">project</a>(point3D);
<a name="l00263"></a>00263         <span class="keywordflow">return</span> p.x()*p.x() + p.y()*p.y();
<a name="l00264"></a>00264         }
<a name="l00265"></a>00265 
<a name="l00266"></a><a class="code" href="group__qvsfm.html#ga55fc8cf62fcfa41cbbed9952aace4556">00266</a> <span class="keywordtype">double</span> reconstructionError(     <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses,
<a name="l00267"></a>00267                                                         <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D,
<a name="l00268"></a>00268                                                         <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointProjections)
<a name="l00269"></a>00269         {
<a name="l00270"></a>00270         <span class="keywordtype">int</span> count = 0;
<a name="l00271"></a>00271         <span class="keywordtype">double</span> error = 0.0;
<a name="l00272"></a>00272         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; pointProjections.size(); i++)
<a name="l00273"></a>00273                 {
<a name="l00274"></a>00274                 error += squaredReprojectionErrorResiduals(points3D[i], cameraPoses, pointProjections[i]);
<a name="l00275"></a>00275                 count += 2 * pointProjections[i].count();
<a name="l00276"></a>00276                 }
<a name="l00277"></a>00277 
<a name="l00278"></a>00278         <span class="keyword">const</span> <span class="keywordtype">double</span> re = sqrt(error / <span class="keywordtype">double</span>(count));
<a name="l00279"></a>00279 
<a name="l00280"></a>00280         <span class="comment">/*#ifdef DEBUG</span>
<a name="l00281"></a>00281 <span class="comment">        const double re2 = reconstructionError2(cameraPoses, points3D, pointProjections);</span>
<a name="l00282"></a>00282 <span class="comment">        Q_ASSERT( ABS(re - re2) / (re + re2) &lt; 1e-5 );</span>
<a name="l00283"></a>00283 <span class="comment">        #endif // DEBUG*/</span>
<a name="l00284"></a>00284 
<a name="l00285"></a>00285         <span class="keywordflow">return</span> re;
<a name="l00286"></a>00286         }
<a name="l00287"></a>00287 
<a name="l00288"></a><a class="code" href="group__qvsfm.html#gaa4ff677564f869aed7cdbce781788cc9">00288</a> <span class="keywordtype">double</span> reconstructionError(     <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses,
<a name="l00289"></a>00289                                                         <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D,
<a name="l00290"></a>00290                                                         <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointProjections,
<a name="l00291"></a>00291                                                         <span class="keyword">const</span> QVector&lt;bool&gt; &amp;evaluateTracking)
<a name="l00292"></a>00292         {
<a name="l00293"></a>00293         Q_ASSERT(pointProjections.count() == evaluateTracking.count());
<a name="l00294"></a>00294         <span class="keywordtype">int</span> count = 0;
<a name="l00295"></a>00295         <span class="keywordtype">double</span> error = 0.0;
<a name="l00296"></a>00296         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; pointProjections.size(); i++)
<a name="l00297"></a>00297                 <span class="keywordflow">if</span> (evaluateTracking[i])
<a name="l00298"></a>00298                         {
<a name="l00299"></a>00299                         error += squaredReprojectionErrorResiduals(points3D[i], cameraPoses, pointProjections[i]);
<a name="l00300"></a>00300                         count += 2 * pointProjections[i].count();
<a name="l00301"></a>00301                         }
<a name="l00302"></a>00302 
<a name="l00303"></a>00303         <span class="keywordflow">if</span> (count &gt; 0)
<a name="l00304"></a>00304                 <span class="keywordflow">return</span> sqrt(error / <span class="keywordtype">double</span>(count));
<a name="l00305"></a>00305         <span class="keywordflow">else</span>
<a name="l00306"></a>00306                 <span class="keywordflow">return</span> 0.0;
<a name="l00307"></a>00307         }
<a name="l00308"></a>00308 
<a name="l00309"></a><a class="code" href="group__qvsfm.html#gafbe2b84faffa62f5838ac22a3b6d2026">00309</a> <span class="keywordtype">double</span> reconstructionError(<span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Rt1, <span class="keyword">const</span> <a class="code" href="classQVMatrix.html" title="Implementation of numerical matrices.">QVMatrix</a> &amp;Rt2, <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D, <span class="keyword">const</span> QVector&lt;QPointFMatching&gt; &amp;matchings)
<a name="l00310"></a>00310         {
<a name="l00311"></a>00311         <span class="keywordtype">double</span> error = 0.0;
<a name="l00312"></a>00312         <span class="keywordtype">int</span> count = 0;
<a name="l00313"></a>00313         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; matchings.size(); i++)
<a name="l00314"></a>00314                 {
<a name="l00315"></a>00315                 <span class="keyword">const</span> <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> &amp;point = points3D[i];
<a name="l00316"></a>00316                 <span class="keyword">const</span> <a class="code" href="group__qvmath.html#ga818fe0c689af956cd14431148c879718">QPointFMatching</a> &amp;matching = matchings[i];
<a name="l00317"></a>00317 
<a name="l00318"></a>00318                 <span class="keyword">const</span> QPointF   p1 = matchings[i].first - Rt1 * point.<a class="code" href="classQVVector.html#acc2df5760c4935341efc23f5b483a5a3" title="Creates vector corresponding to the homogeneous coordinates of a 2D point (appending...">homogeneousCoordinates</a>(),
<a name="l00319"></a>00319                                                 p2 = matchings[i].second - Rt2 * point.<a class="code" href="classQVVector.html#acc2df5760c4935341efc23f5b483a5a3" title="Creates vector corresponding to the homogeneous coordinates of a 2D point (appending...">homogeneousCoordinates</a>();
<a name="l00320"></a>00320 
<a name="l00321"></a>00321                 error += p1.x()*p1.x() + p1.y()*p1.y() + p2.x()*p2.x() + p2.y()*p2.y();
<a name="l00322"></a>00322                 count+=4;
<a name="l00323"></a>00323                 }
<a name="l00324"></a>00324         <span class="keywordflow">return</span> sqrt(error / <span class="keywordtype">double</span>(count));
<a name="l00325"></a>00325         }
<a name="l00326"></a>00326 
<a name="l00327"></a><a class="code" href="group__qvsfm.html#ga17abbe105d91613411f3de1d8cbd9bc3">00327</a> <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> <a class="code" href="group__qvsfm.html#ga17abbe105d91613411f3de1d8cbd9bc3" title="Evaluate the residuals of a reconstruction.">reconstructionErrorResiduals</a>(  <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses,
<a name="l00328"></a>00328                                                                                 <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D,
<a name="l00329"></a>00329                                                                                 <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointTrackings)
<a name="l00330"></a>00330         {
<a name="l00331"></a>00331         QList&lt;double&gt; residuals;
<a name="l00332"></a>00332         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; pointTrackings.size(); i++)
<a name="l00333"></a>00333                 <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, pointTrackings[i].keys())
<a name="l00334"></a>00334                         {
<a name="l00335"></a>00335                         <span class="keyword">const</span> QPointF p = pointTrackings[i][j] -  cameraPoses[j].project(points3D[i]);
<a name="l00336"></a>00336 
<a name="l00337"></a>00337                         residuals &lt;&lt; p.x();
<a name="l00338"></a>00338                         residuals &lt;&lt; p.y();
<a name="l00339"></a>00339                         }
<a name="l00340"></a>00340 
<a name="l00341"></a>00341         <span class="keywordflow">return</span> residuals.toVector();
<a name="l00342"></a>00342         }
<a name="l00343"></a>00343 
<a name="l00344"></a>00344 <span class="comment">// Devuelve TRUE si las poses de cámara contienen un valor NaN</span>
<a name="l00345"></a><a class="code" href="group__qvsfm.html#ga6592d7adcbca1d12882e1f68e3fbb3a9">00345</a> <span class="keywordtype">bool</span> <a class="code" href="group__qvsfm.html#ga6592d7adcbca1d12882e1f68e3fbb3a9" title="Check for NaN values in a list of camera poses.">checkForNaNValues</a>(<span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameraPoses)
<a name="l00346"></a>00346         {
<a name="l00347"></a>00347         <span class="keywordtype">int</span> camerasNaN = 0;
<a name="l00348"></a>00348 
<a name="l00349"></a>00349         <span class="keywordflow">foreach</span>(<a class="code" href="classQVCameraPose.html" title="Class for the camera pose of a view, in a 3D reconstruction.">QVCameraPose</a> cameraPose, cameraPoses)
<a name="l00350"></a>00350                 <span class="keywordflow">if</span>(cameraPose.<a class="code" href="classQVCameraPose.html#a2ab3d874b18ae3b7696388007a096d32" title="Detect NaN values in the components of the camera pose.">containsNaN</a>())
<a name="l00351"></a>00351                         camerasNaN ++;
<a name="l00352"></a>00352 
<a name="l00353"></a>00353         <span class="keywordflow">if</span> (camerasNaN &gt; 0)
<a name="l00354"></a>00354                 std::cout &lt;&lt; <span class="stringliteral">&quot;[checkReconstructionForNaN] Error: found &quot;</span> &lt;&lt; camerasNaN &lt;&lt; <span class="stringliteral">&quot; camera poses containing NaN values&quot;</span> &lt;&lt; std::endl;
<a name="l00355"></a>00355 
<a name="l00356"></a>00356         <span class="keywordflow">return</span> (camerasNaN == 0);
<a name="l00357"></a>00357         }               
<a name="l00358"></a>00358 
<a name="l00359"></a>00359 <span class="comment">// Devuelve TRUE si los puntos 3D contienen un valor NaN</span>
<a name="l00360"></a><a class="code" href="group__qvsfm.html#gaf0ef3cb35d54cb93528315bcd249147a">00360</a> <span class="keywordtype">bool</span> <a class="code" href="group__qvsfm.html#ga6592d7adcbca1d12882e1f68e3fbb3a9" title="Check for NaN values in a list of camera poses.">checkForNaNValues</a>(<span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D)
<a name="l00361"></a>00361         {
<a name="l00362"></a>00362         <span class="keywordtype">int</span> pointsNaN = 0;
<a name="l00363"></a>00363 
<a name="l00364"></a>00364         <span class="keywordflow">foreach</span>(<a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> point, points3D)
<a name="l00365"></a>00365                 <span class="keywordflow">if</span>(point.<a class="code" href="classQVVector.html#a29235517605f49f2327df62d5e0295df" title="Checks whether the vector contains a NaN value or not.">containsNaN</a>())
<a name="l00366"></a>00366                         pointsNaN ++;
<a name="l00367"></a>00367                 
<a name="l00368"></a>00368         <span class="keywordflow">if</span> (pointsNaN &gt; 0)
<a name="l00369"></a>00369                 std::cout &lt;&lt; <span class="stringliteral">&quot;[checkReconstructionForNaN] Error: found &quot;</span> &lt;&lt; pointsNaN &lt;&lt; <span class="stringliteral">&quot; 3D points containing NaN values&quot;</span> &lt;&lt; std::endl;
<a name="l00370"></a>00370 
<a name="l00371"></a>00371         <span class="keywordflow">return</span> (pointsNaN == 0);
<a name="l00372"></a>00372         }               
<a name="l00373"></a>00373 
<a name="l00374"></a>00374 <span class="comment">// Devuelve TRUE si las projecciones contienen un valor NaN</span>
<a name="l00375"></a><a class="code" href="group__qvsfm.html#ga0cbfa4194c9c11fa7a17c81056be0d1e">00375</a> <span class="keywordtype">bool</span> <a class="code" href="group__qvsfm.html#ga6592d7adcbca1d12882e1f68e3fbb3a9" title="Check for NaN values in a list of camera poses.">checkForNaNValues</a>(<span class="keyword">const</span> QList&lt; QHash&lt; int, QPointF &gt; &gt; &amp;pointTrackings)
<a name="l00376"></a>00376         {
<a name="l00377"></a>00377         <span class="keywordtype">int</span> projectionsNaN = 0;
<a name="l00378"></a>00378 
<a name="l00379"></a>00379         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; pointTrackings.count(); i++)
<a name="l00380"></a>00380                 <span class="keywordflow">foreach</span>(<span class="keywordtype">int</span> j, pointTrackings[i].keys())
<a name="l00381"></a>00381                         <span class="keywordflow">if</span> (isnan(pointTrackings[i][j].x()) or isnan(pointTrackings[i][j].y()))
<a name="l00382"></a>00382                                 projectionsNaN++;
<a name="l00383"></a>00383 
<a name="l00384"></a>00384         <span class="keywordflow">if</span> (projectionsNaN &gt; 0)
<a name="l00385"></a>00385                 std::cout &lt;&lt; <span class="stringliteral">&quot;[checkReconstructionForNaN] Error: found &quot;</span> &lt;&lt; projectionsNaN &lt;&lt; <span class="stringliteral">&quot; projections containing NaN values&quot;</span> &lt;&lt; std::endl;
<a name="l00386"></a>00386 
<a name="l00387"></a>00387         <span class="keywordflow">return</span> (projectionsNaN == 0);
<a name="l00388"></a>00388         }
<a name="l00389"></a>00389 
<a name="l00390"></a>00390 <span class="comment">// Returns matchings from A to C.</span>
<a name="l00391"></a>00391 QVector&lt;QVIndexPair&gt; combineMatchingLists(<span class="keyword">const</span> QVector&lt;QVIndexPair&gt; &amp;matchingsAB, <span class="keyword">const</span> QVector&lt;QVIndexPair&gt; &amp;matchingsBC)
<a name="l00392"></a>00392         {
<a name="l00393"></a>00393         QMap&lt;int, int&gt; mapBC;
<a name="l00394"></a>00394         <span class="keywordflow">foreach</span>(QVIndexPair m, matchingsBC)
<a name="l00395"></a>00395                 mapBC[m.first] = m.second;
<a name="l00396"></a>00396 
<a name="l00397"></a>00397         QList&lt;QVIndexPair&gt; matchingsAC;
<a name="l00398"></a>00398         <span class="keywordflow">foreach</span>(QVIndexPair m, matchingsAB)
<a name="l00399"></a>00399                 <span class="keywordflow">if</span> (mapBC.contains(m.second))
<a name="l00400"></a>00400                         matchingsAC &lt;&lt; QVIndexPair(m.first, mapBC[m.second]);
<a name="l00401"></a>00401 
<a name="l00402"></a>00402 <span class="preprocessor">        #ifdef DEBUG</span>
<a name="l00403"></a>00403 <span class="preprocessor"></span>        QSet&lt;int&gt; A, B, C;
<a name="l00404"></a>00404         <span class="comment">// Test for bijection in mapping A&lt;-&gt;B</span>
<a name="l00405"></a>00405         <span class="keywordflow">foreach</span>(QVIndexPair m, matchingsAB)
<a name="l00406"></a>00406                 {
<a name="l00407"></a>00407                 Q_ASSERT(not A.contains(m.first));
<a name="l00408"></a>00408                 Q_ASSERT(not B.contains(m.second));
<a name="l00409"></a>00409                 A &lt;&lt; m.first;
<a name="l00410"></a>00410                 B &lt;&lt; m.second;
<a name="l00411"></a>00411                 }
<a name="l00412"></a>00412 
<a name="l00413"></a>00413         <span class="comment">// Test for bijection in mapping B&lt;-&gt;C</span>
<a name="l00414"></a>00414         B.clear();
<a name="l00415"></a>00415         C.clear();
<a name="l00416"></a>00416         <span class="keywordflow">foreach</span>(QVIndexPair m, matchingsBC)
<a name="l00417"></a>00417                 {
<a name="l00418"></a>00418                 Q_ASSERT(not B.contains(m.first));
<a name="l00419"></a>00419                 Q_ASSERT(not C.contains(m.second));
<a name="l00420"></a>00420                 B &lt;&lt; m.first;
<a name="l00421"></a>00421                 C &lt;&lt; m.second;
<a name="l00422"></a>00422                 }
<a name="l00423"></a>00423 
<a name="l00424"></a>00424         <span class="comment">// Test for bijection in mapping A&lt;-&gt;C</span>
<a name="l00425"></a>00425         A.clear();
<a name="l00426"></a>00426         C.clear();
<a name="l00427"></a>00427         <span class="keywordflow">foreach</span>(QVIndexPair m, matchingsAC)
<a name="l00428"></a>00428                 {
<a name="l00429"></a>00429                 Q_ASSERT(not A.contains(m.first));
<a name="l00430"></a>00430                 Q_ASSERT(not C.contains(m.second));
<a name="l00431"></a>00431                 A &lt;&lt; m.first;
<a name="l00432"></a>00432                 C &lt;&lt; m.second;
<a name="l00433"></a>00433                 }
<a name="l00434"></a>00434 <span class="preprocessor">        #endif // DEBUG </span>
<a name="l00435"></a>00435 <span class="preprocessor"></span>
<a name="l00436"></a>00436         <span class="keywordflow">return</span> matchingsAC.toVector();
<a name="l00437"></a>00437         }
<a name="l00438"></a>00438 
<a name="l00439"></a>00439 <span class="keywordtype">void</span> estimate3DPointsMeanAndVariance(<span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D, <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> &amp;mean, <span class="keywordtype">double</span> &amp;variance)
<a name="l00440"></a>00440         {
<a name="l00441"></a>00441         mean = <a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a>(0.0, 0.0, 0.0);
<a name="l00442"></a>00442 
<a name="l00443"></a>00443         <span class="keywordflow">foreach</span>(<a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> point3D, points3D)
<a name="l00444"></a>00444                 mean = mean + point3D;
<a name="l00445"></a>00445 
<a name="l00446"></a>00446         mean = mean / points3D.count();
<a name="l00447"></a>00447 
<a name="l00448"></a>00448         variance = 0;
<a name="l00449"></a>00449         <span class="keywordflow">foreach</span>(<a class="code" href="classQV3DPointF.html" title="3D point representation">QV3DPointF</a> point3D, points3D)
<a name="l00450"></a>00450                 variance += (point3D - mean) * (point3D - mean);
<a name="l00451"></a>00451 
<a name="l00452"></a>00452         variance = sqrt(variance) / points3D.count();
<a name="l00453"></a>00453         }
<a name="l00454"></a>00454 
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
